#!/usr/bin/env python
"""
Evermore GPS receivers support. Packet encoding/decoding and connection
management.

The EverMore protocol is described in
    EverMore GPS Receiver User Protocol Manual Issue D
Available on the EverMore web site (in the forums).

Apparently the protocol is LITTLE ENDIAN.
"""
import array
import datetime
import math
import struct
import sys
import time

import serial



# Constants
__version__ = 0.1

NUM_LEAP_SECS = 14              # Difference in seconds between GPS time and UTC
RAD_2_DEG = 180. / math.pi
DEG_2_RAD = 1. / RAD_2_DEG
GEOID_ROW = 19
GEOID_COL = 37
GEOID_DELTA = [
    -30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30, -30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30, # 90S
    -53,-54,-55,-52,-48,-42,-38,-38,-29,-26,-26,-24,-23,-21,-19,-16,-12, -8, -4, -1,  1,  4,  4,  6,  5,  4,   2, -6,-15,-24,-33,-40,-48,-50,-53,-52,-53, # 80S
    -61,-60,-61,-55,-49,-44,-38,-31,-25,-16, -6,  1,  4,  5,  4,  2,  6, 12, 16, 16, 17, 21, 20, 26, 26, 22,  16, 10, -1,-16,-29,-36,-46,-55,-54,-59,-61, # 70S
    -45,-43,-37,-32,-30,-26,-23,-22,-16,-10, -2, 10, 20, 20, 21, 24, 22, 17, 16, 19, 25, 30, 35, 35, 33, 30,  27, 10, -2,-14,-23,-30,-33,-29,-35,-43,-45, # 60S
    -15,-18,-18,-16,-17,-15,-10,-10, -8, -2,  6, 14, 13,  3,  3, 10, 20, 27, 25, 26, 34, 39, 45, 45, 38, 39,  28, 13, -1,-15,-22,-22,-18,-15,-14,-10,-15, # 50S
     21,  6,  1, -7,-12,-12,-12,-10, -7, -1,  8, 23, 15, -2, -6,  6, 21, 24, 18, 26, 31, 33, 39, 41, 30, 24,  13, -2,-20,-32,-33,-27,-14, -2,  5, 20, 21, # 40S
     46, 22,  5, -2, -8,-13,-10, -7, -4,  1,  9, 32, 16,  4, -8,  4, 12, 15, 22, 27, 34, 29, 14, 15, 15,  7,  -9,-25,-37,-39,-23,-14, 15, 33, 34, 45, 46, # 30S
     51, 27, 10,  0, -9,-11, -5, -2, -3, -1,  9, 35, 20, -5, -6, -5,  0, 13, 17, 23, 21,  8, -9,-10,-11,-20, -40,-47,-45,-25,  5, 23, 45, 58, 57, 63, 51, # 20S
     36, 22, 11,  6, -1, -8,-10, -8,-11, -9,  1, 32,  4,-18,-13, -9,  4, 14, 12, 13, -2,-14,-25,-32,-38,-60, -75,-63,-26,  0, 35, 52, 68, 76, 64, 52, 36, # 10S
     22, 16, 17, 13,  1,-12,-23,-20,-14, -3, 14, 10,-15,-27,-18,  3, 12, 20, 18, 12,-13, -9,-28,-49,-62,-89,-102,-63, -9, 33, 58, 73, 74, 63, 50, 32, 22, # 00N
     13, 12, 11,  2,-11,-28,-38,-29,-10,  3,  1,-11,-41,-42,-16,  3, 17, 33, 22, 23,  2, -3, -7,-36,-59,-90, -95,-63,-24, 12, 53, 60, 58, 46, 36, 26, 13, # 10N
      5, 10,  7, -7,-23,-39,-47,-34, -9,-10,-20,-45,-48,-32, -9, 17, 25, 31, 31, 26, 15,  6,  1,-29,-44,-61, -67,-59,-36,-11, 21, 39, 49, 39, 22, 10,  5, # 20N
     -7, -5, -8,-15,-28,-40,-42,-29,-22,-26,-32,-51,-40,-17, 17, 31, 34, 44, 36, 28, 29, 17, 12,-20,-15,-40, -33,-34,-34,-28,  7, 29, 43, 20,  4, -6, -7, # 30N
    -12,-10,-13,-20,-31,-34,-21,-16,-26,-34,-33,-35,-26,  2, 33, 59, 52, 51, 52, 48, 35, 40, 33, -9,-28,-39, -48,-59,-50,-28,  3, 23, 37, 18, -1,-11,-12, # 40N
     -8,  8,  8,  1,-11,-19,-16,-18,-22,-35,-40,-26,-12, 24, 45, 63, 62, 59, 47, 48, 42, 28, 12,-10,-19,-33, -43,-42,-43,-29, -2, 17, 23, 22,  6,  2, -8, # 50N
      2,  9, 17, 10, 13,  1,-14,-30,-39,-46,-42,-21,  6, 29, 49, 65, 60, 57, 47, 41, 21, 18, 14,  7, -3,-22, -29,-32,-32,-26,-15, -2, 13, 17, 19,  6,  2, # 60N
      2,  2,  1, -1, -3, -7,-14,-24,-27,-25,-19,  3, 24, 37, 47, 60, 61, 58, 51, 43, 29, 20, 12,  5, -2,-10, -14,-12,-10,-14,-12, -6, -2,  3,  6,  4,  2, # 70N
      3,  1, -2, -3, -3, -3, -1,  3,  1,  5,  9, 11, 19, 27, 31, 34, 33, 34, 33, 34, 28, 23, 17, 13,  9,  4,   4,  1, -2, -2,  0,  2,  3,  2,  1,  1,  3, # 80N
     13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13,  13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13] # 90N
SECS_IN_WEEK = 604800
SECS_IN_DAY = 86400
GPS_EPOCH = (1980, 1, 6, 0, 0, 0)  # (year, month, day, hh, mm, ss)
# GPX formatting
GPX_HEADER = '''<?xml version="1.0" encoding="UTF-8" standalone="no"?>
<gpx xmlns="http://www.topografix.com/GPX/1/1"
     creator="Python EverMode Module"
     version="%s"
     xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
     xsi:schemaLocation="http://www.topografix.com/GPX/1/1/gpx.xsd">
  <metadata>
    <link href="http://www.fpierfed.org/evermore/">
      <text>Francesco Pierfederici</text>
    </link>
    <time>%s</time>
    <bounds
      minlat="%f"
      minlon="%f"
      maxlat="%f"
      maxlon="%f"/>
  </metadata>
  <trk><name>%s</name>
    <trkseg>'''
GPX_TMPL = '''      <trkpt lat="%f" lon="%f">
        <ele>%f</ele>
        <time>%s</time>
      </trkpt>'''
GPX_FOOTER = '''    </trkseg>
  </trk>
</gpx>'''



class DataTransferComplete(Exception):
    """
    Used as signal that the data transfer is indeed complete.
    """
    pass


def sex2dec(sex, sep=':'):
    """
    Convert angles given in sexagesimal format (sex) to decimal format.
    
    Input
    sex     value in sexagesimal format (string)
    sep     separator for degrees, minutes and seconds (char). 
            Defaults to ' '
    
    Return
    Decimal value corresponding to sex (float)
    """
    try:
        (dd, mm, ss) = sex.strip().split(sep)
    except:
        (dd, mm) = sex.strip().split(sep)
        ss = '0'
    if(float(dd) >= 0):
        return(float(dd) + float(mm) / 60.0 + float(ss) / 3600.0)
    return(float(dd) - float(mm) / 60.0 - float(ss) / 3600.0)


def dec2sex (deg, sep=':'):
    """
    Convert angles in decimal format (deg) to sexagesimal format.
    
    Input
    deg     value in decimal format (float)
    sep     separator for degrees, minutes and seconds for output
            formatting (char). Defaults to ' '
    
    Return
    Sexagesimal value corresponding to deg (string)
    """
    deg = float(deg)
    
    degrees = int(deg)
    if(degrees >= 0):
        temp = (deg - degrees) * 60
        minutes = int (temp)
        seconds = int ((temp - minutes) * 60)
    else:
        temp = - (deg - degrees) * 60
        minutes = int (temp)
        seconds = int ((temp - minutes) * 60)
    sex = "%02d%c%02d%c%05.2f" % (degrees, sep, minutes, sep, seconds)
    return(sex)

def dec2bin(n, length=None):
    """
    Convert the positive int n to its binary string representation.
    """
    if(n < 0):
        raise(SyntaxError('dec2bin() only works with positive integers.'))
    elif(n == 0):
        return(_padded('0', length))
    # <-- end if

    s = ''
    while(n > 0):
        s = str(n % 2) + s
        n = n >> 1
    # <-- end while
    return(_padded(s, length))

def _padded(bits, length=None):
    if(not length or length <= len(bits)):
        return(bits)
    # <-- end if
    
    padding = (length - len(bits)) * '0'
    return(padding + bits)

def geoBounds(data):
    """
    Given a list of
        [date, lat, lon, alt, speed, mode, dgps]
    compute the min amd max value for lat and lon
    """
    dates, lats, lons, alts, speeds, modes, dgpss = zip(*data)
    return(min(lats), max(lats), min(lons), max(lons))

def ecef2geo(x, y, z, vx=0., vy=0., vz=0.):
    """
    Given the ECEF (x, y, z) coordinates and optionally the corresponding
    velocity components, compute the longitude, latitude and altitude of the
    same point on the geoid.

    If velocity components are given, compute speed and heading angle as well.

    Code based on geoid.c, part of gpsd code-base.
    """
    a = 6378137                   # equatorial radius
    f = 1 / 298.257223563         # flattening
    b = a * (1 - f)               # polar radius
    e2 = (a*a - b*b) / (a*a)
    e_2 = (a*a - b*b) / (b*b)

    # geodetic location
    lon_rad = math.atan2(y, x)
    
    p = math.sqrt(x**2 + y**2) 
    theta = math.atan2(z * a, p * b)
    lat_rad = math.atan2(z + e_2 * b * math.sin(theta)**3, p - \
                     e2 * a * math.cos(theta)**3)
    n = a / math.sqrt(1. - e2 * math.sin(lat_rad)**2)
    h = p / math.cos(lat_rad) - n
    lat = lat_rad * RAD_2_DEG
    lon = lon_rad * RAD_2_DEG
    alt = h - wgs84_separation(lat, lon)
    
    # velocity computation
    vnorth = -vx * math.sin(lat_rad) * math.cos(lon_rad) - \
             vy * math.sin(lat_rad) * math.sin(lon_rad) + \
             vz * math.cos(lat_rad)
    veast = -vx * math.sin(lon_rad) + vy * math.cos(lon_rad)
    climb = vx * math.cos(lat_rad) * math.cos(lon_rad) + \
            vy * math.cos(lat_rad) * math.sin(lon_rad) + \
            vz * math.sin(lat_rad)
    speed = math.sqrt(vnorth**2 + veast**2)
    heading = math.atan2(veast, vnorth)
    
    if(heading < 0):
        heading += 2 * math.pi
    track = heading * RAD_2_DEG
    return((lat, lon, alt))

def wgs84_separation(lat, lon):
    ilat=int(math.floor((90. + lat) / 10.))
    ilon=int(math.floor((180. + lon) / 10.))
        
    #sanity checks
    if(ilat > 90. or ilat < -90.):
        return(0.)
    if(ilon > 180. or ilon < -180.):
        return(0.)

    ilat1 = ilat
    ilon1 = ilon
    if(ilat < GEOID_ROW- 1 ):
        ilat2 = ilat+1
    else:
        ilat2 = ilat
    # <-- end if
    if(ilon < GEOID_COL- 1 ):
        ilon2 = ilon + 1
    else:
        ilon2 = ilon
    # <-- end if
    return bilinear(
        ilon1*10.-180.,ilat1*10.-90.,
        ilon2*10.-180.,ilat2*10.-90.,
        lon,           lat,
        GEOID_DELTA[ilon1+ilat1*GEOID_COL], 
        GEOID_DELTA[ilon2+ilat1*GEOID_COL],
        GEOID_DELTA[ilon1+ilat2*GEOID_COL], 
        GEOID_DELTA[ilon2+ilat2*GEOID_COL])

def bilinear(x1, y1, x2, y2, x, y, z11, z12, z21, z22):
    if(y1 == y2 and x1 == x2):
        return (z11)
    if(y1 == y2 and x1 != x2):
        return (z22*(x-x1)+z11*(x2-x))/(x2-x1)
    if(x1 == x2 and y1 != y2):
        return (z22*(y-y1)+z11*(y2-y))/(y2-y1)
    delta=(y2-y1)*(x2-x1)
    return (z22 * (y - y1) * (x - x1) + \
            z12 * (y2 - y) * (x - x1) + \
            z21 * (y - y1) * (x2 - x) + \
            z11 * (y2 - y) * (x2 - x)) / delta


def gps2UTC(gpsWeek, SOW, leapSecs=NUM_LEAP_SECS):
    """
    Convert gps week and seconds to UTC

    GPS time is basically measured in (atomic) seconds since 
    January 6, 1980, 00:00:00.0  (the GPS Epoch)
    
    The GPS week starts on Saturday midnight (Sunday morning), and runs
    for 604800 seconds. 

    Currently, GPS time is 14 seconds ahead of UTC.
    While GPS SVs transmit this difference and the date when another leap
    second takes effect, the use of leap seconds cannot be predicted.  This
    routine is precise until the next leap second is introduced and has to be
    updated after that.  

    SOW = seconds of week
    gpsWeek is the full number (not modulo 1024)
    """
    microsec = (SOW % 1) * 1000000
    epochTuple = GPS_EPOCH + (-1, -1, 0)
    # mktime is localtime, correct for UTC
    t0 = time.mktime(epochTuple) - time.timezone
    tdiff = (gpsWeek * SECS_IN_WEEK) + SOW - leapSecs
    t = t0 + tdiff
    
    (year, month, day,
     hh, mm, ss,
     dayOfWeek, julianDay,
     daylightsaving) = time.gmtime(t)
    # Use gmtime since localtime does not allow to switch off daylighsavings
    # correction!!!
    return(datetime.datetime(year, month, day, hh, mm, ss, microsec))

def _decodeVelocity(data):
    """
    Data is an unsigned 16-bit integer made up of
    mode[13] 
    DGPS[12] 
    velocity[9:0]

    Remember that the byte ordering is slipped so in reality all those indeces
    are to be replaced by 15 - index.
    """
    # Turn data into its 16-bit binary representation.
    bits = dec2bin(data, length=16)
    
    speed = int(bits[6:], 2)
    if(bits[2] == '1'):
        mode = '3D'
    else:
        mode = '2D'
    # <-- end if
    if(bits[3] == '1'):
        dgps = 'On'
    else:
        dgps = 'Off'
    # <-- end if
    return(speed, mode, dgps)

def _decodeDate(rawTime, rawDate):
    """
    In reality, of the 2x16-bit of data made up by rawData and rawTime, the
    date is encoded in the first 12 bits and the time in the remaining 20.
    
    Also, date is  a week number, where a year has 52 weeks and week 0 is
    the week of Jan. 06, 1980. Time is the number of seconds since the
    midnight between the previous week's Saturday and this week's Sunday.
    """
    # Decode the GPS week number.
    bits = dec2bin(rawDate, length=16)
    week = int(bits[:12], 2)
    
    # Decode GPS seconds of the week.
    bits = bits[12:] + dec2bin(rawTime, length=16)
    secs = int(bits, 2)

    # Convert to UTC.
    return(gps2UTC(week, secs))



class EverMoreMessage(object):
    """
    A class that encapsulates EverMore GPS messages.
    """
    def __init__(self):
        self._msg = array.array('B')
        return
    
    def decode(self, encodedBytes):
        """
        Decode a stream of bytes representing a EverMore message (and possibly
        some extra data. encodedBytes is an instance of array.array of type 'B'
        """
        msgLength, encodedPayloadFooter = self._decodeHeader(encodedBytes)
        crc, payload, encodedFooter = self._decodeBody(msgLength,
                                                       encodedPayloadFooter)
        extraData = self._decodeFooter(crc, encodedFooter)
        return(payload, extraData)

    def _decodeHeader(self, encodedBytes):
        # Find the beginning of the header (bytes 0x10, 0x02).
        found = False
        for i in range(len(encodedBytes) - 1):
            if(encodedBytes[i] == 0x10 and encodedBytes[i+1] == 0x02):
                # Found the header!
                found = True
                break
            # <-- end if
        # <-- end for

        # Did we find the header?
        if(not found):
            raise(IOError('Header not found.'))
        # <-- end if

        length = encodedBytes[i+2] - 2
        if(length + 2 == 0x10):
            # We need to skip the following byte (also = 0x10).
            return(length, encodedBytes[i+4:])
        return(length, encodedBytes[i+3:])

    def _decodeBody(self, length, encodedBytes):
        payload = array.array('B')
        
        i = 0
        j = 0
        while(j < length):
            payload.append(encodedBytes[i])
            i += 1
            if(payload[-1] == 0x10):
                # skip the next byte.
                i += 1
            # <-- end if
            
            j += 1
        # <-- end while        
        crc = reduce(lambda x, y: x + y, payload) & 0xff
        return(crc, payload, encodedBytes[i:])

    def _decodeFooter(self, crc, encodedBytes):
        if(encodedBytes[0] != crc):
            raise(IOError('Message footer not found!'))
        if(crc == 0x10):
            # Skip the next 3 bytes.
            return(encodedBytes[3:])
        return(encodedBytes[2:])
            
    
    def encode(self, payload):
        """
        Create a message conforming to the EverMore protocol.
        Payload is an instance of array.array made up of unsigned bytes.
        """
        self._msg = array.array('B')
        self._msg += self._encodeHeader(payload)
        self._msg += self._encodeBody(payload)
        self._msg += self._encodeFooter(payload)
        return

    def _encodeHeader(self, payload):
        """
        The header is 3 to 4 bytes in length. It starts with 0x10 followed by
        0x02. The third byte is the payload length + 2, which has to be < 254
        bytes. If the payload length + 2 is 0x10, then the fourth byte has to be
        0x10 as well. Otherwise the 4th byte is skipped.
        """
        assert(len(payload) < 254)

        l = len(payload) + 2
        header = array.array('B')
        header.append(0x10)
        header.append(0x02)
        header.append(l)
        if(l == 0x10):
            header.append(0x10)
        return(header)

    def _encodeFooter(self, payload):
        """
        The footer is made up of three to four bytes. The first byte is the sum
        of the bytes in the payload modulo 256. The second byte is 0x10 is the
        first byte  0x10. Otherwise it is skipped. Byte 3 (or byte 2 if byte 1
        is not 0x10) is 0x10. Byte 4 (or 3 is byte 1 is not 0x10) is 0x03.
        """
        footer = array.array('B')
        
        crc = reduce(lambda x, y: x + y, payload) & 0xff
        footer.append(crc)
        if(footer[0] == 0x10):
            footer.append(0x10)
        footer.append(0x10)
        footer.append(0x03)
        return(footer)

    def _encodeBody(self, payload):
        """
        The payload is sent as is with the exception that is aby byte is 0x10,
        then a second 0x10 is sent after that byte.
        """
        body = array.array('B')
        for byte in payload:
            body.append(byte)
            if(byte == 0x10):
                body.append(0x10)
            # <-- end if
        # <-- end for
        return(body)

    def toString(self):
        return(self._msg.tostring())
        





class EverMoreGPS(object):
    def __init__(self,
                 device='/dev/cu.SLAB_USBtoUART',
                 speed=4800,
                 bytesize=serial.EIGHTBITS,
                 parity=serial.PARITY_NONE,
                 stopbits=serial.STOPBITS_ONE):
        self._device = device
        self._baud = speed
        try:
            self._connection = self._connect()
        except serial.SerialException:
            print('Connection to EverMore GPS receiver failed.')
            sys.exit(1)

        # Some internal vars.
        self._parsedLogData = []
        return

    def _connect(self):
        """
        Open a connection to the self._device virtual file.
        """
        conn = serial.Serial(self._device, self._baud,
                             bytesize=serial.EIGHTBITS,
                             parity=serial.PARITY_NONE,
                             stopbits=serial.STOPBITS_ONE)
        return(conn)

    def read(self, numBytes=None, fromBytes=None):
        """
        Listen for incoming messages. If fromBytes is specified, read until we
        find fromBytes and return the bytes from fromBytes on (including
        fromBytes). If numBytes is specified, read at least nuumBytes bytes.
        """
        rawBytes = array.array('B')
        
        if(numBytes == None and fromBytes == None):
            rawBytes.fromstring(self.readline())
            return(rawBytes)
        elif(fromBytes == None):
            rawBytes.fromstring(self._connection.read(numBytes))
            return(rawBytes)
        else:
            i = 0
            nBytes = len(fromBytes)
            start = None
            while(start == None):
                if(numBytes):
                    rawBytes.fromstring(self._connection.read(numBytes))
                else:
                    rawBytes.fromstring(self.readline())
                # print(rawBytes.tostring())

                for j in range(len(rawBytes)):
                    if(fromBytes == rawBytes[i:i+nBytes].tolist()):
                        start = i
                        break
                    # <-- end if
                    i += 1
                # <-- end for
            # <-- end while

            # We got what we wanted. Just make sure that we got all we wanted.
            if(numBytes and len(rawBytes[start:]) < numBytes):
                # Read the remaining bytes.
                leftToRead = numBytes - len(rawBytes[start:])
                while(leftToRead):
                    rawBytes.fromstring(self._connection.read(leftToRead))
                    leftToRead = numBytes - len(rawBytes[start:])
                # <-- end while
            # <-- end if
            return(rawBytes[start:])
        # <-- end if
        return

    def msg_read(self, proc_function):
        """
        Keep reading data from the serial connection until a messager is
        encountered. When that happens, the message bytes are pased to
        proc_function as the only argument.

        A message is defined as the sequence of bytes between 0x10 0x02 and
        0x10 0x03.
        """
        done = False
        foundHeader = False
        duplicateByte = False
        
        rawMsg = ''
        msg = array.array('B')

        rawByte = self._connection.read(1)
        byte = struct.unpack('B', rawByte)[0]
        while(not done):
            if(not foundHeader and byte == 0x10):
                # This could be the header...
                nextRawByte = self._connection.read(1)
                nextByte = struct.unpack('B', nextRawByte)[0]
                if(nextByte == 0x02):
                    # Found the header!
                    foundHeader = True
                    rawMsg = rawByte + nextRawByte

                    # Read the next byte and keep going.
                    rawByte = self._connection.read(1)
                    byte = struct.unpack('B', rawByte)[0]
                    continue
                # <-- end if

                # Since we did not find the header, update byte amd rawByte.
                byte = nextByte
                rawByte = nextRawByte
                continue
            elif(foundHeader and byte == 0x10):
                # This could be the footer...
                nextRawByte = self._connection.read(1)
                nextByte = struct.unpack('B', nextRawByte)[0]
                if(nextByte == 0x03):
                    # Found the footer!
                    rawMsg += rawByte + nextRawByte

                    # Extract bytes from the message and pass the whole thing to
                    # the user-specified proc_function.
                    msg.fromstring(rawMsg)
                    proc_function(msg)

                    # Re-initialize things and keep going.
                    foundHeader = False
                    rawMsg = ''
                    msg = array.array('B')
                    rawByte = self._connection.read(1)
                    byte = struct.unpack('B', rawByte)[0]
                    continue
                # <-- end if

                # Since we did not find the footer, update byte and rawByte
                # and add the bytes to rawMsg.
                rawMsg += rawByte + nextRawByte
                # <-- end if
                rawByte = self._connection.read(1)
                byte = struct.unpack('B', rawByte)[0]
                continue
            elif(foundHeader and byte != 0x10):
                # Simply add the bytes to msg and read the next byte.
                rawMsg += rawByte
                rawByte = self._connection.read(1)
                byte = struct.unpack('B', rawByte)[0]
                continue
            else:
                # Notthing to see here: move along.
                rawByte = self._connection.read(1)
                byte = struct.unpack('B', rawByte)[0]
                continue
            # <-- end if
        # <-- end while
        return
            
            
    def readline(self, timeout=0):
        """
        Listen for one message.
        """
        return(self._connection.readline())

    def write(self, message):
        """
        Send message to the connected device.
        """
        # Tell the device that we are ready to send.
        # self._connection.setRTS(0)      # We are ready to send
        # self._connection.sendBreak()
        
        # self._connection.flushOutput()
        self._connection.write(message.toString())
        self._connection.drainOutput()

        # We are ready to listen
        # self._connection.setRTS(1)
        return

    def close(self):
        """
        Close the connection to the device.
        """
        self._connection.close()
        return

    def getRevisionInformation(self):
        """
        Retrieve the version number.
        """
        # Ask for rev information.
        msg = EverMoreMessage()
        msg.encode(array.array('B', [0xB3, 0, 0, 0]))
        self.write(msg)

        # Read and decode the answer: keep reading until we find the beginning
        # of the answer packet. 0x10, 0x02 is the start fo the header. The
        # message ID is 0x40 and it has a length 0f 19 bytes total. 19+2 = 0x15
        rawBytes = self.read(numBytes=255, fromBytes=[0x10, 0x02, 0x15, 0x40])

        # Decode the packet.
        payload, theRest = EverMoreMessage().decode(rawBytes)

        # Pad it and unpack the whole thing.
        payload.insert(1, 0x00)
        payload.insert(1, 0x00)
        payload.insert(1, 0x00)

        (msgID,
         date,
         release,
         releaseType,
         appCode,
         ignored) = struct.unpack('BxxxL10sssH', payload.tostring())
        return(date, release, releaseType + appCode)
        
    def getLogStatus(self, password=0):
        """
        Retrieve the Log configuration.

        In case the device is password protected, the correct password has to be
        specified and not null/0. The password is a positive integer of up to 8
        digits. The password is sent in clear.
        """
        # The password is a unsigned 32 bit integer...
        passwdBytes = struct.unpack('4B',
                                    array.array('L',
                                                [int(password)]).tostring())
        # Ask for log status.
        msg = EverMoreMessage()
        msg.encode(array.array('B', [0x81, 0x13, 0x00, 0x00] + \
                                     list(passwdBytes)))

        # for byte in msg._msg:
        #     sys.stdout.write('%s ' %(hex(byte)))
        # sys.stdout.write('\n')
        self.write(msg)

        # Read and decode the answer: keep reading until we find the beginning
        # of the answer packet.
        rawBytes = self.read(numBytes=255, fromBytes=[0x10, 0x02, 0x1d, 0x20])

        # Decode the packet.
        payload, theRest = EverMoreMessage().decode(rawBytes)

        # Extract the info from the decoded bytes.
        # The packet structure is such that we need to add one byte of padding
        # after the message id (i.e. at byte 1).
        payload.insert(1, 0x00)

        # Now we are ready to unpack the payload.
        (msgID,
         percentFull,
         config,
         dtMin,
         dtMax,
         ddMin,
         numPoints,
         login,
         passwdBytes,
         uniqueID) = struct.unpack('BxHHHHHLLLL', payload.tostring())
        
        # Extract the config info.
        config = dec2bin(config)
        if(config[0] == '0'):
            loggingEnabled = False
        else:
            loggingEnabled = True
        # <-- end if
        if(config[1] == '0'):
            distanceUnits = 'ft'
        else:
            distanceUnits = 'm'
        # <-- end if
        

        # Percent full is in unites of 0.01%
        percentFull *= 0.01

        # Return the info (excluding login and passwd).
        return(numPoints, percentFull, loggingEnabled, distanceUnits,
               dtMin, dtMax, ddMin)

    def eraseLogDataBuffer(self, password=0):
        """
        Erase the content of the lo data buffer of the GPS device.

        In case the device is password protected, the correct password has to be
        specified and not null/0. The password is a positive integer of up to 8
        digits. The password is sent in clear.
        """
        # The password is a unsigned 32 bit integer...
        passwdBytes = struct.unpack('4B',
                                    array.array('L',
                                                [int(password)]).tostring())
        
        msg = EverMoreMessage()
        msg.encode(array.array('B', [0x81, 0x11, 0x00, 0x00] + \
                                     list(passwdBytes)))
        self.write(msg)
        return

    def getLogData(self, numPoints=None, units='m', password=0):
        """
        Retrieve the whole logged data.

        In case the device is password protected, the correct password has to be
        specified and not null/0. The password is a positive integer of up to 8
        digits. The password is sent in clear.
        """
        self._connection.setTimeout(None)
        
        # Initialize self._parsedLogData
        self._parsedLogData = []

        # The password is a unsigned 32 bit integer...
        passwdBytes = struct.unpack('4B',
                                    array.array('L',
                                                [int(password)]).tostring())
        
        # Ask a log data dump at high speed (19200 bps). Also set a timeout of
        # 1 second.
        logOutput = int('10000000', 2)
        msg = EverMoreMessage()
        msg.encode(array.array('B', [0x81, 0x12, 0x00, logOutput] + \
                                     list(passwdBytes)))
        self.write(msg)
        # self._connection._baudrate = 19200
        # self._connection._timeout = 20
        # self._connection._reconfigurePort()
        
        # Read and parse all the data.
        try:
            self.msg_read(self._parseLogData)
        except DataTransferComplete:
            pass

        # Restore the standard port speed (4800 bps).
        # self._connection._baudrate = 4800
        # self._connection._timeout = 0
        # self._connection._reconfigurePort()

        # Now that we are done reading the data, print it out in GPX format.
        return(self._formatLogData())

    def _formatLogData(self, trackName='PyEverMore'):
        """
        Print the log data stored in self._parsedLogData in GPX format.
        """
        gpx = ''
        
        # Compute the lon/lat bounds.
        (minLat, maxLat, minLon, maxLon) = geoBounds(self._parsedLogData)

        # Get the current datetime
        now = datetime.datetime.now()

        gpx += GPX_HEADER %(__version__,
                            now.isoformat(),
                            minLat,
                            minLon,
                            maxLat,
                            maxLon,
                            trackName) + '\n'
        for (dateTime, lat, lon, alt, speed, mode, dgps) in self._parsedLogData:
            utc = dateTime.isoformat() + 'Z'
            gpx += GPX_TMPL % (lat, lon, alt, utc) + '\n'
        # <-- end for
        gpx += GPX_FOOTER  + '\n'
        return(gpx)
        
        

    def _parseLogData(self, rawBytes):
        """
        Decode the input rawBytes array and print out the decoded message.
        """
        try:
            payload, theRest = EverMoreMessage().decode(rawBytes)
        except:
            print('*** The following message appears to be invalid:')
            print('*** ' + str([hex(x) for x in rawBytes]).replace("'", ""))
            return
        # The payload is made up of
        # - one unsigned 16-bit integer (velocity)
        # - two unsigned 16-bit integers (date)
        # - one signed 32-bit integer (EFEC X)
        # - one signed 32-bit integer (EFEC Y)
        # - one signed 32-bit integer (EFEC Z)

        # Skip the first byte (message type).
        payload = payload[1:]

        # The payload for the log data messages is at least 18 bytes (plus the
        # one byte for the message ID that we have discared).
        if(len(payload) != 18):
            # Wrong message.
            return

        # Python wants longs at 4 byte boundaries...
        payload.insert(6, 0x00)
        payload.insert(6, 0x00)
    
        rawV, rawT, rawD, x, y, z = struct.unpack('3H3l', payload.tostring())
        speed, mode, dgps = _decodeVelocity(rawV)
        dateTime = _decodeDate(rawT, rawD)
        lat, lon, alt = ecef2geo(x, y, z)

        # Ok, this is a HACK! I somehow cannot find a reliable way to tell when
        # all the log data has been tranferred. So, what I am doing is this: if
        # the data we are trying to store is the same as the last one already
        # stored, we assume that the data stream is over.
        if(self._parsedLogData and dateTime == self._parsedLogData[-1][0]):
            raise(DataTransferComplete())
        # <-- end if
        
        # Add the data to the self._parsedData array.
        self._parsedLogData.append((dateTime,
                                    lat,
                                    lon,
                                    alt,
                                    speed,
                                    mode,
                                    dgps))
        
        # print('%s: %s %s %.02fm %dm/s (mode %s, DGPS %s)' \
        #  %(str(dateTime), dec2sex(lat), dec2sex(lon), alt, speed, mode, dgps))
        return

    def setMode(self, mode='nmea'):
        """
        Set the protocol mode: binary or NMEA.
        """
        VALID_MODES = ('binary', 'nmea')

        mode = mode.lower()
        if(mode not in VALID_MODES):
            return
        
        # Change the mode.
        msg = EverMoreMessage()
        if(mode == 'binary'):
            msg.encode(array.array('B', [0x84, 0x00, 0x00, 0x00]))
        else:
            msg.encode(array.array('B', [0x84, 0x01, 0x00, 0x00]))
        # <-- end if
        
        self.write(msg)
        return(mode)

    def setDGPS(self, mode='on', type='sbas', timeout=0, satellite=1):
        """
        Turns DGPS on/off

        Type can be either 'sbas' or 'rtcm'

        In case of RTCM, a timeout can be specified.

        In case of SBAS, a satellite ID has to be chosen:
        0: WAAS  POR  (134) US Pacific region
        1: WAAS  AOR-W(122) US Atlantic Region
        2: EGNOS AOR-E(120) Europe
        3: EGNOS IOR  (131) Europe
        4: MTSAT-1    (129) Japan
        5: MTSAT-2    (137) Japan

        In theory it would be possible to use RTCM instead if the default SBAS
        but the GT-6000 is not able to support it.
        """
        VALID_MODES = ('on', 'off')
        VALID_TYPES = ('sbas', 'rtcm')

        mode = mode.lower()
        if(mode not in VALID_MODES):
            return

        typ = type.lower()
        if(typ not in VALID_TYPES):
            return        

        # Change the DGPS mode.
        msg = EverMoreMessage()
        if(mode == 'on' and typ == 'sbas'):
            modeByte = 0x01
            timeout = [0x00, 0x00]
        elif(mode == 'on' and typ == 'rtcm'):
            modeByte = 0x02
            satellite = 0x00
            tmp = array.array('H', [timeout])
            timeout = list(struct.unpack('BB', tmp.tostring()))
        else:
            modeByte = 0x00
            satellite = 0x00
            timeout = [0x00, 0x00]
        # <-- end if

        
        msg.encode(array.array('B', [0x94, modeByte] + \
                               timeout + \
                               [satellite, 0x00, 0x00, 0x00]))
        # <-- end if
        self.write(msg)
        return(mode)

    def resetToDefaults(self):
        # Reset to defaults.
        msg = EverMoreMessage()
        msg.encode(array.array('B', [0x95, 0x01, 0x00, 0x00]))
        # <-- end if

        self.write(msg)
        return

    def testConnection(self, timeout=1):
        # Set the connection timeout to 1 second. Try to read from the port, if
        # it fails, the device is not connected.
        self._connection.setTimeout(timeout)
        data = self._connection.read(1)
        return(bool(data))

        
if(__name__ == '__main__'):
    gps = EverMoreGPS()

    # gps.resetToDefaults()

    # print(gps.getRevisionInformation())

    # print(gps.getLogStatus())

    # print(gps.getLogData())

    print(gps.eraseLogDataBuffer())
    
    # print(gps.setDGPS(mode='on', type='rtcm', timeout=10))
    # print(gps.setDGPS(mode='on', type='sbas', satellite=0))
    # print(gps.setDGPS(mode='off'))

    # import array
    # import struct
    # import numpy
    # bytes = array.array('B', [0x22, 0x9f, 0xe2, 0xff,
    #                           0x9d, 0x0a, 0xb3, 0xff,
    #                           0x45, 0xac, 0x33, 0x00])
    # coords = [numpy.array([float(x), ]) \
    #                       for x in struct.unpack('3l', bytes.tostring())]
    # print(ecef2geo(*coords))

    # m = array.array('B', [0x10, 0x02, 0x15, 0x22, 0x05, 0xa0, 0x65, 0x98, 0x96,
    #                       0x5a, 0x22, 0x9f, 0xe2, 0xff, 0x9d, 0x0a, 0xb3, 0xff,
    #                       0x45, 0xac, 0x33, 0x00, 0xd3, 0x10, 0x03])
    # _parseLogData(m)
